/**
 * File: communication.cpp
 * Brief:
 * Created by: zhangping
 * Created at: 2017.05.3
 * Modified by:
 * Modified at:
 **/
 
#include <iostream>
#include "kinematic.h"


namespace kinematic{


float myabs(float val)
{
	return val>0?val:-val;
}

/**
 * Function: LinearVelocityToPulse
 * Brief:
 * Args:
 *     vel: linear velocity, unit is 'm/s'.
 * Return:
 *     the number of pulse, unit is 'number/s'.
 */
U16 LinearVelocityToPulse(float vel)
{
    vel = myabs(vel);
    float f_num = vel*100000/(9*kPi);
    U16 u_num = 0;
    if(f_num>65535){ // 每秒脉冲数量限制
        std::cerr<<"Error: 每秒线脉冲数量超出65535"<<std::endl;
        u_num = 65535;
    }else{
        u_num = f_num;
    }

    return u_num;
}


/**
 * Function: RotationVelocityToPluse
 * Brief:
 * Args:
 *     vel: rotation velocity, unit is 'rad/s'.
 * Return:
 *     the number of pulse, unit is 'number/s'.
 */
U16 RotationVelocityToPluse(float vel)
{
    vel = myabs(vel);
    //float f_num = vel*3549; // test value
    float f_num = vel*100000/(9*kPi);
    U16 u_num = 0;
    
    if(f_num>65535){ // 每秒脉冲数量限制
        std::cerr<<"Error: 每秒旋转脉冲数量超出65535"<<std::endl;
        u_num = 65535;
    }else{
        u_num = f_num;
    }
    
    return u_num;
}


/**
 * Function: ComputeDirection
 * Brief: 计算速度控制位。
 * Args:
 *     vel_x: velocity at x-axis，move toward the right.
 *     vel_y: velocity at y-axis, move forward.
 *     vel_w: rotation velocity.
 * Return:
 *     direction.
 */
U8 ComputeDirection(float vel_x, float vel_y, float vel_w)
{
	U8 direction = 0;
    if(vel_x!=0 && vel_x<0){
        direction |= 0x04;
	}
	
    if(vel_y!=0 && vel_y<0){
        direction |= 0x02;
	}

    if(vel_w!=0 && vel_w<0){
        direction |= 0x01;
	}

	return direction;
}


} // end of namespace
